home *** CD-ROM | disk | FTP | other *** search
/ Graphics Plus / Graphics Plus.iso / libs / viewwrld / viewwrld.lha / viewworld / lgd / vector.c < prev    next >
Encoding:
C/C++ Source or Header  |  1989-09-21  |  6.4 KB  |  254 lines
  1. /* $Id: vector.c,v 1.7 89/09/20 17:49:22 mbp Exp $
  2.  *
  3.  * vector.c: vector procedures
  4.  */
  5.  
  6. /************************************************************************
  7.  *        Copyright (C) 1989 by Mark B. Phillips                  *
  8.  *                                     *
  9.  * Permission to use, copy, modify, and distribute this software and    *
  10.  * its documentation for any purpose and without fee is hereby granted, *
  11.  * provided that the above copyright notice appear in all copies and    *
  12.  * that both that copyright notice and this permission notice appear in *
  13.  * supporting documentation, and that the name of Mark B. Phillips or   *
  14.  * the University of Maryland not be used in advertising or publicity   *
  15.  * pertaining to distribution of the software without specific, written *
  16.  * prior permission.  This software is provided "as is" without express *
  17.  * or implied warranty.                                                 *
  18.  ************************************************************************/
  19.  
  20. #include    <math.h>
  21. #include    "lgd.h"
  22. #include    "internal.h"
  23. #include    "vector.h"
  24.  
  25. #define        PI        3.1415927
  26. #define        DEGTORAD(x)    (PI*x/180)
  27. #define        YES        1
  28. #define        NO        0
  29.  
  30.  
  31. /* NOTE: All vectors in this file are assumed to have dimension
  32.  *   lgd_dim. */
  33.  
  34. /*-----------------------------------------------------------------------
  35.  * Function:    LGD_copy_vec
  36.  * Description:    vector copy operation
  37.  * Args  IN:    v2: source
  38.  *      OUT:    v1: destination
  39.  */
  40. LGD_copy_vec( v1, v2 )
  41. double v1[],v2[];
  42. {
  43.   int i;
  44.  
  45.   for (i=0; i<lgd_dim; ++i) v1[i] = v2[i];
  46. }
  47.  
  48.  
  49. /*-----------------------------------------------------------------------
  50.  * Function:    LGD_add_vec
  51.  * Description:    vector addition
  52.  * Args  IN:    v2,v3: summands
  53.  *      OUT:    v1: the sum v2 + v3
  54.  */
  55. LGD_add_vec( v1, v2, v3 )
  56. double v1[],v2[],v3[];
  57. {
  58.   int i;
  59.   
  60.   for (i=0; i<lgd_dim; ++i)
  61.     v1[i] = v2[i] + v3[i];
  62. }
  63.  
  64. /*-----------------------------------------------------------------------
  65.  * Function:    LGD_sub_vec
  66.  * Description:    vector subtraction
  67.  * Args  IN:    v2,v3: vectors to be subtracted
  68.  *      OUT:    v1: the difference v2 - v3
  69.  */
  70. LGD_sub_vec( v1, v2, v3 )
  71. double v1[], v2[], v3[];
  72. {
  73.   int i;
  74.   for (i=0; i<lgd_dim; ++i)
  75.     v1[i] = v2[i] - v3[i];
  76. }
  77.  
  78. /*-----------------------------------------------------------------------
  79.  * Function:    LGD_L2norm_vec
  80.  * Description:    L2 (Euclidean) norm of a vector
  81.  * Args  IN:    v: a vector
  82.  * Returns:    it's norm
  83.  */
  84. double
  85. LGD_L2norm_vec( v )
  86. double v[];
  87. {
  88.   return( sqrt( LGD_dotprod_vec( v, v ) ) );
  89. }
  90.  
  91. /*-----------------------------------------------------------------------
  92.  * Function:    LGD_L2dist_vec
  93.  * Description:    L2 (Euclidean) distance between vectors
  94.  * Args  IN:    v1,v2: two vectors
  95.  * Returns:    the distance between them
  96.  */
  97. double
  98. LGD_L2dist_vec( v1, v2 )
  99. double v1[], v2[];
  100. {
  101.   double v[3];
  102.   LGD_sub_vec( v, v1, v2 );
  103.   return( LGD_L2norm_vec( v ) );
  104. }
  105.  
  106.  
  107. /*-----------------------------------------------------------------------
  108.  * Function:    LGD_dotprod_vec
  109.  * Description:    dot product (regular old dot product)
  110.  * Args  IN:    v1,v2: two vectors
  111.  * Returns:    their dot product
  112.  */
  113. double
  114. LGD_dotprod_vec( v1, v2 )
  115. double v1[], v2[];
  116.  
  117. {
  118.   int i;
  119.   double val;
  120.   for (i=0, val=0;  i<lgd_dim;  ++i)
  121.     val += v1[i] * v2[i];
  122.   return(val);
  123. }
  124.  
  125.  
  126. /*-----------------------------------------------------------------------
  127.  * Function:    LGD_unit_vec
  128.  * Description:    unitize a vector
  129.  * Args  IN:    v: a vector
  130.  *      OUT:    v: unit vector having same direction as original v
  131.  */
  132. LGD_unit_vec( v )
  133. double v[];
  134. {
  135.   LGD_scamul_vec( v, 1.0/LGD_L2norm_vec( v ), v );
  136. }
  137.  
  138.  
  139. /*-----------------------------------------------------------------------
  140.  * Function:    LGD_scamul_vec
  141.  * Description:    multiply a vector by a scalar
  142.  * Args  IN:    s: a scalar
  143.  *        v2: a vector
  144.  *      OUT:    v1: s times v2
  145.  */
  146. LGD_scamul_vec( v1, s, v2 )
  147. double v1[], s, v2[];
  148. {
  149.   int i;
  150.   for (i=0; i<lgd_dim; ++i)
  151.     v1[i] = s*v2[i];
  152. }
  153.  
  154. /*-----------------------------------------------------------------------
  155.  * Function:    LGD_mult_matrix4x4
  156.  * Description:    multiply two 4x4 matrices
  157.  * Args  IN:    B,C: the factors
  158.  *      OUT:    A: the product B*C
  159.  */
  160. LGD_mult_matrix4x4(A, B, C)
  161.      double A[4][4], B[4][4], C[4][4];
  162. {
  163.   register int i,j,k;
  164.  
  165.   for (i=0; i<4; ++i)
  166.     for (j=0; j<4; ++j) {
  167.       A[i][j] = 0;
  168.       for (k=0; k<4; ++k)
  169.     A[i][j] += B[i][k] * C[k][j];
  170.     }
  171. }
  172.  
  173. /*-----------------------------------------------------------------------
  174.  * Function:    LGD_matmul_vec
  175.  * Description:    multiply a (3x3) matrix by a vector
  176.  * Args  IN:    M: a 3x3 matrix
  177.  *        v2: a vector
  178.  *      OUT:    v1: the product M*v2
  179.  */
  180. LGD_matmul_vec( v1, M, v2 )
  181. double v1[3], M[3][3], v2[3];
  182. {
  183.   int i,j;
  184.   for (i=0; i<3; ++i) {
  185.     v1[i] = 0;
  186.     for (j=0; j<3; ++j)
  187.       v1[i] += M[i][j]*v2[j];
  188.   }
  189. }
  190.  
  191.  
  192. /*-----------------------------------------------------------------------
  193.  * Function:    LGD_cross_vec
  194.  * Description:    vector cross product
  195.  * Args  IN:    v2,v3: two vectors
  196.  *      OUT:    v1: the cross produc v2 x v3
  197.  */
  198. LGD_cross_vec( v1, v2, v3 )
  199. double v1[], v2[], v3[];
  200. {
  201.   v1[0] = v2[1] * v3[2]  -  v2[2] * v3[1];
  202.   v1[1] = v2[2] * v3[0]  -  v2[0] * v3[2];
  203.   v1[2] = v2[0] * v3[1]  -  v2[1] * v3[0];
  204. }
  205.  
  206. /*-----------------------------------------------------------------------
  207.  * Function:    LGD_compute_3d_rot_mat
  208.  * Description:    compute 3x3 rotation matrix about axis through origin
  209.  * Args  IN:    u: direction of axis of rotation
  210.  *        deg: angle of rotation, in degrees
  211.  *      OUT:    M: the rotation matrix
  212.  * Notes:   1.    Positive angles give counterclockwise rotation as
  213.  *        seen from the end of u looking towards the origin.
  214.  *        2.  u need not be a unit vector
  215.  */
  216. LGD_compute_3d_rot_mat( M, u, deg )
  217.      double M[3][3], u[3], deg;
  218. {
  219.   double unit_u[3], costh, sinth, u1sq, u2sq, u3sq;
  220.   double u1u2, u1u3, u2u3, one_minus_costh;
  221.   
  222. #define    u1    unit_u[0]
  223. #define u2    unit_u[1]
  224. #define u3    unit_u[2]
  225.   
  226.   LGD_copy_vec( unit_u, u );
  227.   LGD_unit_vec( unit_u );
  228.   costh = cos( DEGTORAD(deg) );
  229.   sinth = sin( DEGTORAD(deg) );
  230.   one_minus_costh = 1 - costh;
  231.   u1sq = u1 * u1;
  232.   u2sq = u2 * u2;
  233.   u3sq = u3 * u3;
  234.   u1u2 = u1 * u2;
  235.   u1u3 = u1 * u3;
  236.   u2u3 = u2 * u3;
  237.   /* row 1: */
  238.   M[0][0] = u1sq + costh * ( 1 - u1sq );
  239.   M[0][1] = u1u2 * one_minus_costh - u3*sinth;
  240.   M[0][2] = u1u3 * one_minus_costh + u2*sinth;
  241.   /* row 2: */
  242.   M[1][0] = u1u2 * one_minus_costh + u3*sinth;
  243.   M[1][1] = u2sq + costh * ( 1 - u2sq );
  244.   M[1][2] = u2u3 * one_minus_costh - u1*sinth;
  245.   /* row 3: */
  246.   M[2][0] = u1u3 * one_minus_costh - u2*sinth;
  247.   M[2][1] = u2u3 * one_minus_costh + u1*sinth;
  248.   M[2][2] = u3sq + costh * ( 1 - u3sq );
  249.   
  250. #undef    u1
  251. #undef    u2
  252. #undef    u3
  253. }
  254.